#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/nonfree/nonfree.hpp>
#include "mytest.h"

int video_test()
{
    std::string fileName="test.mp4";
    cv::VideoCapture capture(fileName);
    if(!capture.isOpened())
    {  std::cerr << "Failed to open video file" << std::endl; return 1; }
    
    cv::Mat frame;
    double fps = capture.get(CV_CAP_PROP_FPS);
    
    std::string windowName = "Video Test";
    cv::namedWindow(windowName, CV_WINDOW_NORMAL);
    while(true)
    {
      capture >> frame;
      cv::imshow(windowName, frame);
      cv::waitKey(1000/fps);
    }
}

void my_videotemplateMatch(cv::Mat &temp, int method, bool bResize)
{
    std::string fileName="testHD.mp4";
    cv::VideoCapture capture(fileName);
    if(!capture.isOpened())
    {  std::cerr << "Failed to open video file" << std::endl; return; }
    
    cv::Mat frame, frame_bin, result;
    double fps = capture.get(CV_CAP_PROP_FPS);
    
    std::string windowName = "Video Test";
    cv::namedWindow(windowName, CV_WINDOW_NORMAL);
    int skipFrames=5, recommended_width = 800, recommended_height=600;
    int count=skipFrames;
    int thresh = 127, kernelSize=3;
    double requiredConfidence=0.93;
    cv::Scalar color(255, 0, 0);
    double minVal, maxVal; 
    cv::Point minLoc, maxLoc, matchPoint;
    my_convertToBinary(temp, temp);
    my_openImage(temp, kernelSize);
//     cv::Canny(temp, temp, 200, 20);
    while(true)
    {
      capture >> frame;
      if(bResize) cv::resize(frame, frame, cv::Size(recommended_width, recommended_height));
      if(++count >= skipFrames)
      {
	//maxVal = 0;
	frame_bin = frame;
	my_convertToGray8Bit(frame_bin); //convert to gray
	cv::equalizeHist(frame_bin, frame_bin); //equalize hist
	my_convertToBinary(frame_bin, frame_bin, thresh); //thresholding
	my_openImage(frame_bin, kernelSize);
// 	cv::Canny(frame_bin, frame_bin, 200, 20);
//  	my_templateMatch(frame_bin, temp, result, method); //template matching
	my_templateMatchMultiScale(frame_bin, temp, result, method, requiredConfidence);
// 	my_templateMatchPyramid(frame_bin, temp, result, 3, requiredConfidence);
	
	count = 0;
	cv::minMaxLoc(result, &minVal, &maxVal, &minLoc, &maxLoc, cv::Mat());
	matchPoint = (method < 2) ? minLoc : maxLoc;
      }
	//my_convertToBinary(frame, frame, thresh);
      if(maxVal > requiredConfidence)
	cv::rectangle(frame, matchPoint, cv::Point(matchPoint.x+temp.cols, matchPoint.y+temp.rows), color, 2, 8, 0);
      cv::imshow(windowName, frame);
      cv::waitKey(1000/fps);
    }
}
  
void my_videotemplateMatchSIFT(cv::Mat &temp, int method, bool bResize) { std::string fileName="testHD.mp4";
    cv::VideoCapture capture(fileName);
    if(!capture.isOpened())
    {  std::cerr << "Failed to open video file" << std::endl; return; }
    
    cv::Mat frame, frame_bin, result;
    double fps = capture.get(CV_CAP_PROP_FPS);
    
    std::string windowName = "Video Test";
    cv::namedWindow(windowName, CV_WINDOW_NORMAL);
    int skipFrames=5, recommended_width = 800, recommended_height=600;
    int count=skipFrames;
//     int thresh = 127, kernelSize=3;
//     double requiredConfidence=0.93;
    cv::Scalar color(255, 0, 0);
    double minVal, maxVal; 
    cv::Point minLoc, maxLoc, matchPoint;
//     my_convertToBinary(temp, temp);
//     my_openImage(temp, kernelSize);
//     cv::Canny(temp, temp, 200, 20);
    my_convertToGray8Bit(temp);
    cv::FastFeatureDetector SIFTDetector(10);
    std::vector<cv::KeyPoint>  keypoints_temp, keypoints_frame;
    SIFTDetector.detect(temp, keypoints_temp);
    cv::SiftDescriptorExtractor SIFTExtractor;
    cv::Mat descriptors_temp, descriptors_frame;
    SIFTExtractor.compute(temp, keypoints_temp, descriptors_temp);
//     cv::BFMatcher matcher;
    cv::FlannBasedMatcher matcher;
    std::vector<cv::DMatch> matches;
  std::vector<cv::Point2f> scene_corners(4);
  bool bFound;
  
    while(true)
    {
      capture >> frame;
      if(bResize) cv::resize(frame, frame, cv::Size(recommended_width, recommended_height));
	frame_bin = frame;
      if(++count >= skipFrames)
      {
	count = 0; bFound = false;
	my_convertToGray8Bit(frame); //convert to gray
	cv::equalizeHist(frame, frame); //equalize hist
	SIFTDetector.detect(frame, keypoints_frame);
	SIFTExtractor.compute(frame, keypoints_frame, descriptors_frame);
	matcher.match(descriptors_temp, descriptors_frame, matches);
	  //-- Quick calculation of max and min distances between keypoints
	 double max_dist = 0; double min_dist = 100;
      for( int i = 0; i < descriptors_temp.rows; i++ )
      { double dist = matches[i].distance;
	if( dist < min_dist ) min_dist = dist;
	if( dist > max_dist ) max_dist = dist;
      }

  //-- Draw only "good" matches (i.e. whose distance is less than 3*min_dist )
  std::vector< cv::DMatch > good_matches;

  for( int i = 0; i < descriptors_temp.rows; i++ )
  { 
    if( matches[i].distance < 5*min_dist )
     { good_matches.push_back( matches[i]); }
  }
  //good_matches = matches;
	if(good_matches.size() >= 5)
	{ 
	  bFound = true;
	
	std::vector<cv::Point2f> obj;
	std::vector<cv::Point2f> scene;
	for( int i = 0; i < good_matches.size(); i++ )
	{
	  //-- Get the keypoints from the good matches
	  obj.push_back( keypoints_temp[ good_matches[i].queryIdx ].pt );
	  scene.push_back( keypoints_frame[ good_matches[i].trainIdx ].pt );
	}
	cv::Mat H = cv::findHomography(obj, scene, CV_RANSAC, 10);
	
	//-- Get the corners from the image_1 ( the object to be "detected" )
	std::vector<cv::Point2f> obj_corners(4);
	obj_corners[0] = cv::Point2f(0,0); obj_corners[1] = cv::Point2f( temp.cols, 0 );
	obj_corners[2] = cv::Point2f( temp.cols, temp.rows ); obj_corners[3] = cv::Point2f( 0, temp.rows );

	perspectiveTransform( obj_corners, scene_corners, H);
	}	
      }
      if(bFound) {
  cv::line( frame_bin, scene_corners[0] , scene_corners[1]  , cv::Scalar(255, 0, 0), 2 );
  cv::line( frame_bin, scene_corners[1] , scene_corners[2]  , cv::Scalar( 255, 0, 0), 2 );
  cv::line( frame_bin, scene_corners[2] , scene_corners[3]  , cv::Scalar( 255, 0, 0), 2 );
  cv::line( frame_bin, scene_corners[3] , scene_corners[0]  , cv::Scalar( 255, 0, 0), 2 );
      }
	//my_convertToBinary(frame, frame, thresh);
//       if(maxVal > requiredConfidence)
//  	cv::rectangle(frame_bin, matchPoint, cv::Point(matchPoint.x+temp.cols, matchPoint.y+temp.rows), color, 2, 8, 0);
      //cv::drawMatches(temp, keypoints1, frame, keypoints2, matches, frame_bin);
      cv::imshow(windowName, frame_bin);
      cv::waitKey(1000/fps);
    }
}